/* USER CODE BEGIN Header */
/**
 ******************************************************************************
 * @file           : main.c
 * @brief          : Main program body
 ******************************************************************************
 * @attention
 *
 * Copyright (c) 2024 STMicroelectronics.
 * All rights reserved.
 *
 * This software is licensed under terms that can be found in the LICENSE file
 * in the root directory of this software component.
 * If no LICENSE file comes with this software, it is provided AS-IS.
 *
 ******************************************************************************
 */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "dma.h"
#include "fdcan.h"
#include "gpio.h"
#include "tim.h"
#include "usart.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */

#include "log.h"
#include "motor_cybergear.h"
#include "printf.h"
#include "stdlib.h"
#include "string.h"
#include "test.h"
#include "unity.h"

/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */

/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/

/* USER CODE BEGIN PV */
// uint8_t rx1_buffer[RX_BUFFER_MAX_SIZE];
/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
/* USER CODE BEGIN PFP */

/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
#define RX_BUFFER_MAX_SIZE 8
uint8_t rx_buffer[RX_BUFFER_MAX_SIZE];
MotorMIT_Typedef motor_mit;
MotorStatus_Typedef motor_status;

void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
    static float t = 0.0f;
    static float freq = 2.0f, amplitude = 2.8f;
    float iq;
    if (htim->Instance == TIM3)
    {
        //        motor_mit.forward_torque = 0.0f;
        //        motor_mit.kp = 480.0f;
        //        motor_mit.ki = 4.8f;
        //        t += 0.002f;
        //        motor_mit.target_position = -0.5f * amplitude * cosf(M_2PI_F * freq * t) + 0.5f * amplitude;
        //        motor_mit.target_velocity = 0.5f * amplitude * M_2PI_F * freq * sinf(M_2PI_F * freq * t);
        //        motor_control_mit(0x01, &motor_mit);
        //        motor_set_position(0x01, motor_mit.target_position);
        iq = amplitude * cosf(motor_status.actual_position - 0.6283185f);
        motor_set_current(0x01, iq);
    }
}

void UART1_IdleCallback(UART_HandleTypeDef *huart)
{
    static uint16_t current_ndtr;
    static uint16_t data_length;
    if (huart->Instance == USART1)
    {
        __HAL_UART_CLEAR_IDLEFLAG(huart); // 清除空闲中断标志
        current_ndtr = huart->hdmarx->Instance->CNDTR;
        data_length = RX_BUFFER_MAX_SIZE - current_ndtr;

        // user code, example: echo
        HAL_UART_Transmit_DMA(&huart1, rx_buffer, data_length);

        huart->hdmarx->Instance->CMAR = (uint32_t)rx_buffer;
        huart->hdmarx->Instance->CNDTR = RX_BUFFER_MAX_SIZE;
        HAL_UART_Receive_DMA(huart, rx_buffer, RX_BUFFER_MAX_SIZE);
    }
}

void HAL_FDCAN_RxFifo0Callback(FDCAN_HandleTypeDef *hfdcan, uint32_t RxFifo0ITs)
{
    static FDCAN_RxHeaderTypeDef can_rx_header;
    static uint8_t RxData[8];

    if (RxFifo0ITs & FDCAN_IT_RX_FIFO0_NEW_MESSAGE)
    {
        if (HAL_FDCAN_GetRxMessage(hfdcan, FDCAN_RX_FIFO0, &can_rx_header, RxData) != HAL_OK)
        {
            Error_Handler();
        }

        //! user code, example: echo to huart1
        if (can_rx_header.Identifier >> 24 == 2)
        {
            motor_status = motor_get_status(can_rx_header.Identifier, RxData);

            //            printf("%.3f, %.3f, %.3f, %.3f, %.3f\r\n", motor_mit.target_position,
            //            motor_mit.target_velocity,
            //                   motor_status.actual_position, motor_status.actual_velocity,
            //                   motor_status.actual_torque);
            printf("%.3f, %.3f\r\n", motor_mit.target_position, motor_status.actual_position);
            //            printf("%.3f, %.3f\r\n", motor_mit.target_velocity, motor_status.actual_velocity);
            //            HAL_UART_Transmit_DMA(&huart1, RxData, 8);
        }
    }
}

/* USER CODE END 0 */

/**
 * @brief  The application entry point.
 * @retval int
 */
int main(void)
{
    /* USER CODE BEGIN 1 */

    /* USER CODE END 1 */

    /* MCU Configuration--------------------------------------------------------*/

    /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
    HAL_Init();

    /* USER CODE BEGIN Init */

    /* USER CODE END Init */

    /* Configure the system clock */
    SystemClock_Config();

    /* USER CODE BEGIN SysInit */

    /* USER CODE END SysInit */

    /* Initialize all configured peripherals */
    MX_GPIO_Init();
    MX_DMA_Init();
    MX_FDCAN1_Init();
    MX_USART1_UART_Init();
    MX_TIM2_Init();
    MX_TIM3_Init();
    /* USER CODE BEGIN 2 */

    UNITY_BEGIN();
    HAL_TIM_Base_Start_IT(&htim2);
    HAL_UART_Receive_DMA(&huart1, rx_buffer, RX_BUFFER_MAX_SIZE);
    __HAL_UART_ENABLE_IT(&huart1, UART_IT_IDLE);
    HAL_Delay(1000);

    MotorPID_Typedef motor_pid;

    motor_pid.cur_filt_gain = 0.9f;
    motor_pid.cur_kp = 0.025f;
    motor_pid.cur_ki = 0.0258f;
    motor_pid.spd_kp = 10.0f;
    motor_pid.spd_ki = 0.15f;
    motor_pid.loc_kp = 200.0f;
    motor_pid.spd_filt_gain = 0.5f;

    motor_disable(0x01);
    HAL_Delay(100);

    motor_set_pid(0x01, &motor_pid);
    HAL_Delay(100);
    motor_set_zero(0x01);
    HAL_Delay(100);
    motor_enable(0x01);
    HAL_Delay(100);

    //    motor_set_mode(0x01, MODE_MIT);
    //    HAL_Delay(100);
    //    motor_set_mode(0x01, MODE_POS);
    //    HAL_Delay(100);
    motor_set_mode(0x01, MODE_TORQUE);
    HAL_Delay(100);

    HAL_TIM_Base_Start_IT(&htim3);

    /* USER CODE END 2 */

    /* Infinite loop */
    /* USER CODE BEGIN WHILE */
    while (1)
    {
        //                run_test();
        //        HAL_GPIO_TogglePin(LED_GPIO_Port, LED_Pin);
        HAL_Delay(1000);
        /* USER CODE END WHILE */

        /* USER CODE BEGIN 3 */
    }
    /* USER CODE END 3 */
}

/**
 * @brief System Clock Configuration
 * @retval None
 */
void SystemClock_Config(void)
{
    RCC_OscInitTypeDef RCC_OscInitStruct = {0};
    RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

    /** Configure the main internal regulator output voltage
     */
    HAL_PWREx_ControlVoltageScaling(PWR_REGULATOR_VOLTAGE_SCALE1);

    /** Initializes the RCC Oscillators according to the specified parameters
     * in the RCC_OscInitTypeDef structure.
     */
    RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI;
    RCC_OscInitStruct.HSIState = RCC_HSI_ON;
    RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
    RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
    RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI;
    RCC_OscInitStruct.PLL.PLLM = RCC_PLLM_DIV4;
    RCC_OscInitStruct.PLL.PLLN = 75;
    RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
    RCC_OscInitStruct.PLL.PLLQ = RCC_PLLQ_DIV2;
    RCC_OscInitStruct.PLL.PLLR = RCC_PLLR_DIV2;
    if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
    {
        Error_Handler();
    }

    /** Initializes the CPU, AHB and APB buses clocks
     */
    RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2;
    RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
    RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
    RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV1;
    RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;

    if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_4) != HAL_OK)
    {
        Error_Handler();
    }
}

/* USER CODE BEGIN 4 */

/* USER CODE END 4 */

/**
 * @brief  This function is executed in case of error occurrence.
 * @retval None
 */
void Error_Handler(void)
{
    /* USER CODE BEGIN Error_Handler_Debug */
    /* User can add his own implementation to report the HAL error return state */
    __disable_irq();
    while (1)
    {
    }
    /* USER CODE END Error_Handler_Debug */
}

#ifdef USE_FULL_ASSERT
/**
 * @brief  Reports the name of the source file and the source line number
 *         where the assert_param error has occurred.
 * @param  file: pointer to the source file name
 * @param  line: assert_param error line source number
 * @retval None
 */
void assert_failed(uint8_t *file, uint32_t line)
{
    /* USER CODE BEGIN 6 */
    /* User can add his own implementation to report the file name and line number,
       ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
    /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */
